/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <voxel_grid/voxel_grid.h>
#include <gtest/gtest.h>

TEST(voxel_grid, basicMarkingAndClearing)
{
    int size_x = 50, size_y = 10, size_z = 16;
    voxel_grid::VoxelGrid vg(size_x, size_y, size_z);

    //Put a "tabletop" into the scene.  A flat rectangle of set voxels at z = 12.
    int table_z = 12;
    int table_x_min = 5, table_x_max = 15;
    int table_y_min = 0, table_y_max = 3;
    for (int x = table_x_min; x <= table_x_max; x++)
    {
        vg.markVoxelLine(x, table_y_min, table_z, x, table_y_max, table_z);
    }

    for (int i = table_x_min; i <= table_x_max; ++i)
    {
        for (int j = table_y_min; j <= table_y_max; ++j)
        {
            //check that each cell of the table is marked
            ASSERT_EQ(voxel_grid::MARKED, vg.getVoxel(i, j, table_z));
        }
    }

    int mark_count = 0;
    unsigned int unknown_count = 0;
    //go through each cell in the voxel grid and make sure that only 44 are filled in
    for (unsigned int i = 0; i < vg.sizeX(); ++i)
    {
        for (unsigned int j = 0; j < vg.sizeY(); ++j)
        {
            for (unsigned int k = 0; k < vg.sizeZ(); ++k)
            {
                if (vg.getVoxel(i, j, k) == voxel_grid::MARKED) mark_count++;
                else if (vg.getVoxel(i, j, k) == voxel_grid::UNKNOWN) unknown_count++;
            }
        }
    }
    ASSERT_EQ(mark_count, 44);
    //the rest of the cells should be unknown
    ASSERT_EQ(unknown_count, vg.sizeX() * vg.sizeY() * vg.sizeZ() - 44);
    //now, let's clear one of the rows of the table
    vg.clearVoxelLine(table_x_min, table_y_min, table_z, table_x_max, table_y_min, table_z);
    mark_count = 0;
    unknown_count = 0;
    int free_count = 0;
    //go through each cell in the voxel grid and make sure that only 33 are now filled in
    for (unsigned int i = 0; i < vg.sizeX(); ++i)
    {
        for (unsigned int j = 0; j < vg.sizeY(); ++j)
        {
            for (unsigned int k = 0; k < vg.sizeZ(); ++k)
            {
                if (vg.getVoxel(i, j, k) == voxel_grid::MARKED) mark_count++;
                else if (vg.getVoxel(i, j, k) == voxel_grid::FREE) free_count++;
                else if (vg.getVoxel(i, j, k) == voxel_grid::UNKNOWN) unknown_count++;
            }
        }
    }
    //we've switched 11 cells from marked to free
    ASSERT_EQ(mark_count, 33);
    //we've just explicitly seen through 11 cells
    ASSERT_EQ(free_count, 11);
    //the rest of the cells should still be unknown
    ASSERT_EQ(unknown_count, vg.sizeX() * vg.sizeY() * vg.sizeZ() - 44);
    //now let's put in a vertical column manually to test markVoxel
    for (unsigned int i = 0; i < vg.sizeZ(); ++i)
    {
        vg.markVoxel(0, 0, i);
        ASSERT_EQ(vg.getVoxel(0, 0, i), voxel_grid::MARKED);
    }
    vg.printColumnGrid();
    vg.printVoxelGrid();
    //now, let's clear that line of voxels and make sure that they clear out OK
    vg.clearVoxelLine(0, 0, 0, 0, 0, vg.sizeZ() - 1);
    for (unsigned int i = 0; i < vg.sizeZ(); ++i)
    {
        ASSERT_EQ(vg.getVoxel(0, 0, i), voxel_grid::FREE);
    }
    mark_count = 0;
    // Visualize the output
    /*
    v->printVoxelGrid();
    v->printColumnGrid();
    printf("CostMap:\n===========\n");
    for (int y = 0; y < size_y; y++)
    {
        for (int x = 0; x < size_x; x++)
        {
            printf((costMap[y * size_x + x] > 0 ? "#" : " "));
        }
        printf("|\n");
    }
    */

}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}
